﻿using System;
using System.Diagnostics;
using System.Numerics;
using CommunityToolkit.Mvvm.ComponentModel;
using MauiControl.Converters;

namespace MauiControl.Models
{
    public partial class SensorData : ObservableObject
    {
        private readonly QuaternionFusionConverter _quaternionFusionConverter = new QuaternionFusionConverter();

        // 设备原始数据
        [ObservableProperty]
        private double _accelX;

        [ObservableProperty]
        private double _accelY;

        [ObservableProperty]
        private double _accelZ;

        [ObservableProperty]
        private double _angularVelX;

        [ObservableProperty]
        private double _angularVelY;

        [ObservableProperty]
        private double _angularVelZ;

        // 实时四元数
        [ObservableProperty]
        private double _quaternionW;

        [ObservableProperty]
        private double _quaternionX;

        [ObservableProperty]
        private double _quaternionY;

        [ObservableProperty]
        private double _quaternionZ;

        // 实时欧拉角
        [ObservableProperty]
        private double _eulerX;

        [ObservableProperty]
        private double _eulerY;

        [ObservableProperty]
        private double _eulerZ;

        // 融合四元数
        [ObservableProperty]
        private double _fusedQuaternionW;

        [ObservableProperty]
        private double _fusedQuaternionX;

        [ObservableProperty]
        private double _fusedQuaternionY;

        [ObservableProperty]
        private double _fusedQuaternionZ;

        // 融合欧拉角
        [ObservableProperty]
        private double _fusedEulerX;

        [ObservableProperty]
        private double _fusedEulerY;

        [ObservableProperty]
        private double _fusedEulerZ;

        // 红点位置
        [ObservableProperty]
        private Point _redDotPosition = new Point(0, 0);

        // 偏移量（用于重置标点）
        [ObservableProperty]
        private Point _offset = new Point(0, 0);

        // 更新传感器数据
        public void UpdateSensorData(SensorData newData)
        {
            // 检查传入的数据是否有效
            if (newData == null)
            {
                Console.WriteLine("传入的 SensorData 为 null");
                return;
            }

            // 检查加速度计数据是否有效
            if (double.IsNaN(newData.AccelX) || double.IsInfinity(newData.AccelX) ||
                double.IsNaN(newData.AccelY) || double.IsInfinity(newData.AccelY) ||
                double.IsNaN(newData.AccelZ) || double.IsInfinity(newData.AccelZ))
            {
                Console.WriteLine("加速度计数据无效");
                return;
            }

            // 检查陀螺仪数据是否有效
            if (double.IsNaN(newData.AngularVelX) || double.IsInfinity(newData.AngularVelX) ||
                double.IsNaN(newData.AngularVelY) || double.IsInfinity(newData.AngularVelY) ||
                double.IsNaN(newData.AngularVelZ) || double.IsInfinity(newData.AngularVelZ))
            {
                Console.WriteLine("陀螺仪数据无效");
                return;
            }

            // 更新原始数据
            AccelX = newData.AccelX;
            AccelY = newData.AccelY;
            AccelZ = newData.AccelZ;

            AngularVelX = newData.AngularVelX;
            AngularVelY = newData.AngularVelY;
            AngularVelZ = newData.AngularVelZ;

            QuaternionW = newData.QuaternionW;
            QuaternionX = newData.QuaternionX;
            QuaternionY = newData.QuaternionY;
            QuaternionZ = newData.QuaternionZ;

            EulerX = newData.EulerX;
            EulerY = newData.EulerY;
            EulerZ = newData.EulerZ;

            // 使用融合算法计算四元数和欧拉角
            _quaternionFusionConverter.UpdateQuaternion(
                (float)AngularVelX, (float)AngularVelY, (float)AngularVelZ,
                (float)AccelX, (float)AccelY, (float)AccelZ
            );

            var fusedQuaternion = _quaternionFusionConverter.GetQuaternion();
            var fusedEuler = _quaternionFusionConverter.CalculateEulerAngles();

            FusedQuaternionW = fusedQuaternion[0];
            FusedQuaternionX = fusedQuaternion[1];
            FusedQuaternionY = fusedQuaternion[2];
            FusedQuaternionZ = fusedQuaternion[3];

            FusedEulerX = fusedEuler[0];
            FusedEulerY = fusedEuler[1];
            FusedEulerZ = fusedEuler[2];

            // 更新红点位置
            UpdateRedDotPosition();
        }

        // 更新红点位置
        private void UpdateRedDotPosition()
        {
            // 检查融合欧拉角是否有效
            if (double.IsNaN(FusedEulerX) || double.IsInfinity(FusedEulerX) ||
                double.IsNaN(FusedEulerY) || double.IsInfinity(FusedEulerY))
            {
                Console.WriteLine("融合欧拉角无效，无法更新红点位置");
                return;
            }

            // 将新的欧拉角度映射到红点位置
            double yaw = FusedEulerZ; // Yaw (Z轴旋转，左右转动)
            double pitch = FusedEulerY; // Pitch (Y轴旋转，上下转动)

            // 计算红点位置（考虑偏移量）
            double x = yaw - Offset.X; // Yaw 控制 X 轴
            double y = -pitch + Offset.Y; // Pitch 控制 Y 轴（取反以匹配坐标系）

            Console.WriteLine($"更新红点位置: X={x}, Y={y}");

            // 更新红点位置
            RedDotPosition = new Point(x, y);
        }

        // 重置标点
        public void ResetOffset()
        {
            // 获取芯片的当前状态
            string chipState = _quaternionFusionConverter.GetChipState((float)AccelX, (float)AccelY, (float)AccelZ);
            Console.WriteLine($"芯片状态: {chipState}");
            Debug.WriteLine($"读取到的传感器数据: {chipState}");

            // 将当前的 Yaw 和 Pitch 角度设置为偏移量
            Offset = new Point(FusedEulerZ, -FusedEulerY);

            // 重置红点到 (0, 0)
            RedDotPosition = new Point(0, 0);

            Console.WriteLine($"重置标点: Offset={Offset}, RedDotPosition={RedDotPosition}");
        }
    }
}